/* i2c - Simple example

   Simple I2C example that shows how to initialize I2C
   as well as reading and writing from and to registers for a sensor connected over I2C.

   The sensor used in this example is a MPU9250 inertial measurement unit.

   For other examples please check:
   https://github.com/espressif/esp-idf/tree/master/examples

   See README.md file to get detailed usage of this example.

   This example code is in the Public Domain (or CC0 licensed, at your option.)

   Unless required by applicable law or agreed to in writing, this
   software is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
   CONDITIONS OF ANY KIND, either express or implied.
*/
#include <stdio.h>
#include "esp_log.h"
#include "driver/i2c.h"
#include <string.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/event_groups.h"

#include "mpu6050.h"

static const char *TAG = "i2c-simple-example";

#define I2C_MASTER_SCL_IO           15      /*!< GPIO number used for I2C master clock */
#define I2C_MASTER_SDA_IO           18      /*!< GPIO number used for I2C master data  */
#define I2C_MASTER_NUM              0                          /*!< I2C master i2c port number, the number of i2c peripheral interfaces available will depend on the chip */
#define I2C_MASTER_FREQ_HZ          400000                     /*!< I2C master clock frequency */
#define I2C_MASTER_TX_BUF_DISABLE   0                          /*!< I2C master doesn't need buffer */
#define I2C_MASTER_RX_BUF_DISABLE   0                          /*!< I2C master doesn't need buffer */
#define I2C_MASTER_TIMEOUT_MS       1000

#define MPU9250_SENSOR_ADDR                 0x68        /*!< Slave address of the MPU9250 sensor */
#define MPU9250_WHO_AM_I_REG_ADDR           0x75        /*!< Register addresses of the "who am I" register */

#define MPU9250_PWR_MGMT_1_REG_ADDR         0x6B        /*!< Register addresses of the power managment register */
#define MPU9250_RESET_BIT                   7

/**
 * @brief i2c master initialization
 */
static esp_err_t i2c_master_init(void)
{
    int i2c_master_port = I2C_MASTER_NUM;

    i2c_config_t conf = {
        .mode = I2C_MODE_MASTER,
        .sda_io_num = I2C_MASTER_SDA_IO,
        .scl_io_num = I2C_MASTER_SCL_IO,
        .sda_pullup_en = GPIO_PULLUP_ENABLE,
        .scl_pullup_en = GPIO_PULLUP_ENABLE,
        .master.clk_speed = I2C_MASTER_FREQ_HZ,
    };

    i2c_param_config(i2c_master_port, &conf);

    return i2c_driver_install(i2c_master_port, conf.mode, I2C_MASTER_RX_BUF_DISABLE, I2C_MASTER_TX_BUF_DISABLE, 0);
}

uint8_t getCirclePos(double angle)
{
	//角度从-180°到180°，一圈360°分成36格，每转过10°算一格	
	return (uint8_t)((angle - (-180.0)) / 10);
}

void app_main(void)
{
	double maxangle[2] = {0}, minangle[2] = {0};;	
	uint32_t mintimestamp[2] = {0}, maxtimestamp[2] = {0};
	double rolltemp,pitchtemp;	
	double currentangel[2] = {0};
	uint8_t tracedir[2] = {0};
	uint8_t lasttracedir[2] = {0};
	uint32_t movecnt = {0};
	uint8_t moveflag[2] = {0};
	uint8_t levell[2] = {0}, levelh[2] = {0};
	uint8_t state = 0;
		
	MPU6050_t mdata;
    i2c_master_init();
	
	MPU6050_Init();
	MPU6050_Read_All(&mdata,&rolltemp,&pitchtemp);
	
	currentangel[0] = rolltemp;
	currentangel[1] = pitchtemp;
	maxangle[0] = minangle[0] = currentangel[0];
	maxangle[1] = minangle[1] = currentangel[1];
	mintimestamp[0] = maxtimestamp[0] = xTaskGetTickCount() * portTICK_PERIOD_MS;
	mintimestamp[1] = maxtimestamp[1] = xTaskGetTickCount() * portTICK_PERIOD_MS;
	levell[0] = 60;
	levelh[0] = 150;
	levell[1] = 10;
	levelh[1] = 90;

	while(1)
	{
		vTaskDelay(100/portTICK_PERIOD_MS);
		
		MPU6050_Read_All(&mdata,&rolltemp,&pitchtemp);
		currentangel[0] = rolltemp;
		currentangel[1] = pitchtemp;
		ESP_LOGI(TAG, "angle:%f,%f", rolltemp, pitchtemp);

		for(uint8_t k = 0;k < 2;k++) {
			if(currentangel[k] > maxangle[k]) {//如果当前角度大于记录的最大角度，则更新最大角度和相应时间戳
				maxangle[k] = currentangel[k];
				maxtimestamp[k] = xTaskGetTickCount()*portTICK_PERIOD_MS;
			}
			if(currentangel[k] < minangle[k]) {//如果当前角度小于记录的最小角度，则更新最小角度和相应时间戳
				minangle[k] = currentangel[k];
				mintimestamp[k] = xTaskGetTickCount()*portTICK_PERIOD_MS;
			}
			if(maxangle[k] - minangle[k] > levell[k] && maxangle[k] - minangle[k] < levelh[k]) {//如果最大角度和最小角度相差在20°到60°之间，则可能是有效运动
				if(abs(maxtimestamp[k] - mintimestamp[k]) > 10 && abs(maxtimestamp[k] - mintimestamp[k]) < 10000) {//判断最大最小时间戳，防止运动太快或太慢
					if(maxtimestamp[k] > mintimestamp[k]) {//判断运动趋势，即向前运动或向后运动
						tracedir[k] = 1;
					} else {
						tracedir[k] = 2;
					}
					if((lasttracedir[k] == 1 && tracedir[k] == 2) || (lasttracedir[k] == 2 && tracedir[k] == 1)) {
						ESP_LOGI(TAG, "tracedir:%d,%d", k, tracedir[k]);
						moveflag[k]++;
					}
				}
				else {
					//ESP_LOGI(TAG, "time gap fail:%d", abs(maxtimestamp - mintimestamp));
					moveflag[k] = 0;
					state = 0;
				}
				lasttracedir[k] = tracedir[k];//保存本次运动趋势，并更新最大最小角度和时间戳，进行新一轮判断
				if(maxtimestamp[k] > mintimestamp[k]) {
					mintimestamp[k] = maxtimestamp[k];
					minangle[k] = maxangle[k];
				} else {
					maxtimestamp[k] = mintimestamp[k];
					maxangle[k] = minangle[k];
				}
			}
		}
		//将转圈运动划分为4个阶段，俯仰角持续变大（变小）中，翻转角都要经历一次运动方向反向的过程
		switch(state) {
			case 0:
				if(moveflag[0] == 0 && moveflag[1] == 1) {
					ESP_LOGI(TAG, "state:%d", state);
					state++;
				}
				break;
			case 1:
				if(moveflag[0] == 1 && moveflag[1] == 1) {
					ESP_LOGI(TAG, "state:%d", state);
					state++;
				}
				break;
			case 2:
				if(moveflag[0] == 1 && moveflag[1] == 2) {
					ESP_LOGI(TAG, "state:%d", state);
					state++;
				}
				break;
			case 3:
				if(moveflag[0] == 2 && moveflag[1] == 2) {
					ESP_LOGI(TAG, "state:%d", state);
					state++;
				}
				break;
			case 4:
				movecnt++;				
				ESP_LOGI(TAG, "move cnt:%d", movecnt);
				memset(moveflag, 0, sizeof(moveflag));
				state = 0;
				break;
		}
	}
}
